import trimesh
import numpy as np


class Vertex():

    def __init__(self, vertex):
        self.vertex = vertex

    def move(self, direction, length):
        point=(self.vertex + np.array(direction)*length)
        return point

    def get_vertex(self):
        return self.vertex


class Edge():

    def __init__(self, origin, direction=None, length=None, end=None):
        self.origin = np.array(origin)
        if direction is None:
            self.direction = None
        else:
            self.direction = np.array(direction)

        if length is None:
            self.length = None
        else:
            self.length = length

        if end is None:
            self.end = None
        else:
            self.end = np.array(end)

    def get_edge(self):
        if self.end is None:
            v1_vertex = Vertex(self.origin)
            v1 = v1_vertex.get_vertex()
            v2 = v1_vertex.move(self.direction,self.length)
            edge = np.hstack((v1, v2)).reshape(2, 3)

        elif self.end is not None:
            edge = np.hstack((self.origin,self.end)).reshape(2, 3)

        return edge

    def show(self):
        ray_visualize=trimesh.load_path(self.get_edge())
        scene = trimesh.Scene([ray_visualize])
        scene.show()


class Path():
    def __init__(self, origin, *args, **kwargs):
        self.origin = np.array(origin)
        self.args = args
        self.kwargs = kwargs

    def build_edge(self):
        edge_list = []
        if len(self.args)>0:
            e1=Edge(self.origin,end=self.args[0]).get_edge()
            edge_list.append(e1)
            for i in range(len(self.args)-1):
                edge=Edge(self.args[i],end=self.args[i+1]).get_edge()
                edge_list.append(edge)

        elif len(self.args)==0 and self.kwargs is not None:
            assert self.kwargs["movements"] is not None
            moves = self.kwargs["movements"]
            directions = moves["directions"]
            lengths = moves["lengths"]
            #e1=Edge(self.origin,directions[0],lengths[0]).get_edge()
            points=[]
            points.append(self.origin)
            assert len(directions) == len(lengths)
            for i in range(len(directions)):
                #points.append()
                edge = Edge(points[i],directions[i],lengths[i]).get_edge()
                v = Vertex(points[i])
                points.append(v.move(direction=directions[i],length=lengths[i]))
                edge_list.append(edge)

        return edge_list

    def get_path(self):
        # print(self.args)
        # print(self.args[0])
        # print(len(self.args))
        # print(self.kwargs)
        path = np.array(self.build_edge())
        return path

    def show(self):
        ray_visualize=trimesh.load_path(self.get_path())
        scene = trimesh.Scene([ray_visualize])
        scene.show()

p=Path([0, 0, 0],movements={"directions":[[1,0,0],[0,1,0],[0,0,1],[1,0,0]],"lengths":[5,10,3,20]})
#print(p.build_edge())
print(p.get_path())
p.show()



